using System;
using System.Collections.Generic;
using System.Text;
using Noein.Physics;
using Microsoft.DirectX;

namespace Noein.Control
{

    class FlockController : Controller
    {
        // acceleration at which the boids move toward their goal
        private float acceleration = 5;
        public float Acceleration
        {
            get { return acceleration; }
            set { acceleration = value; }
        }

        #region Weights

        private float collision;
        public float CollisionWeight
        {
            get { return collision; }
            set { collision = value; }
        }

        private float centering;
        public float CenteringWeight
        {
            get { return centering; }
            set { centering = value; }
        }

        private float velocityMatching;
        public float VelocityMatchingWeight
        {
            get { return velocityMatching; }
            set { velocityMatching = value; }
        }
        
        private float follow;
        public float FollowWeight
        {
            get { return follow; }
            set { follow = value; }
        }

        #endregion
        
        // neighborhood detection
        private float range;
        private float rangeSq;
        public float Range
        {
            get { return range; }
            set { range = value; rangeSq = range * range; }
        }

        private float degreeAngle;
        private float cosAngle;
        public float Angle
        {
            get { return degreeAngle; }
            set { degreeAngle = value; cosAngle = (float)System.Math.Cos(degreeAngle * (float)System.Math.PI / 180f); }
        }

        // number of boids
        private int capacity;
        public int NBoids
        {
            get { return capacity; }
            set { capacity = value; updateCapacity = true; }
        }
        private bool updateCapacity = false;

        private ParticleController controller = new ParticleController();

        private Random rand = new Random();

        public FlockController()
        {
            controller.AddForce(new Friction(1, 1));
        }

        public override void Update(Object obj, float dt)
        {
            //throw new Exception("The method or operation is not implemented.");
            if (Pause)
                return;

            ParticleList target = obj as ParticleList;

            if (updateCapacity)
                ReallocatePool(target);

            // compute normalized weights
            float total = collision + centering + velocityMatching + follow;
            float nCollision, nCentering, nVelocityMatching, nFollow;
            if (total == 0)
            {
                nCollision = 0;
                nCentering = 0;
                nVelocityMatching = 0;
                nFollow = 0;
            }
            else
            {
                nCollision = collision / total;
                nCentering = centering / total;
                nVelocityMatching = velocityMatching / total;
                nFollow = follow / total;
            }

            foreach (ParticleList.Particle boid in target.Particles)
            {
                List<ParticleList.Particle> neighbors = GetNeighbors(boid, target);

                // HACK use pre and current position to estimate leader velocity
                //Vector3 targetVelocity = target.Spatial.Position - target.Spatial.PreviousPosition;
                //targetVelocity *= 1 / dt;

                float forceMagnitude = boid.Physical.Mass * acceleration;

                // apply following force in direction of velocity difference
                Vector3 totalForce =
                    nFollow * forceMagnitude *
                    Vector3.Normalize(target.Spatial.Position - boid.Physical.Position);
                    //targetVelocity - boid.Physical.Velocity);
                
                if (neighbors.Count != 0)
                {
                    Vector3 averagePosition = Vector3.Empty;
                    Vector3 averageVelocity = Vector3.Empty;
                    foreach (ParticleList.Particle neighbor in neighbors)
                    {
                        averagePosition += neighbor.Spatial.Position;
                        averageVelocity += boid.Physical.Velocity;
                    }
                    float oneOverCount = 1 / neighbors.Count;
                    averagePosition *= oneOverCount;
                    averageVelocity *= oneOverCount;

                    // apply centering & collision force relative to direction of average position
                    totalForce +=
                        (nCentering - nCollision) * forceMagnitude *
                         Vector3.Normalize(averagePosition - boid.Spatial.Position);

                    // apply velocity matching force
                    totalForce +=
                        nVelocityMatching * forceMagnitude *
                         Vector3.Normalize(averageVelocity - boid.Physical.Velocity);
                }

                boid.Physical.CustomForce = totalForce;
            }
        }

        public override void AttachTo(Object obj)
        {
            ParticleList target = obj as ParticleList;

            if (target == null)
                throw new Exception("Attempt to attach object of none particle type to flock controller!");

            if (obj.Spatial == null)
                obj.Spatial = new Spatial();

            if (obj.Physical == null)
                throw new Exception("Attempt to attach objects without physics to a flock!");

            obj.Controllers.Add(this);

            ReallocatePool(target);
        }

        public override void Reset(Object obj)
        {
            base.Reset(obj);
        }

        private void ReallocatePool(ParticleList target)
        {
            while (target.Particles.Count < capacity)
            {
                target.Particles.AddLast(
                    new LinkedListNode<ParticleList.Particle>(new ParticleList.Particle()));
                target.Particles.Last.Value.Controllers.Add(controller);
                target.Particles.Last.Value.Spatial = new Spatial();

                // random place boid around leader
                target.Particles.Last.Value.Spatial.TeleportTo(new Vector3(
                    target.Spatial.Position.X + rand.Next(-5, 5),
                    target.Spatial.Position.Y + rand.Next(-5, 5),
                    target.Spatial.Position.Z + rand.Next(-5, 5)));

                target.Particles.Last.Value.Physical = new Physical(target.Particles.Last.Value.Spatial);
                target.Particles.Last.Value.Physical.Mass = 1;

                // make visible
                target.Particles.Last.Value.Age = 0;
                target.Particles.Last.Value.LifeTime = 1;
            }

            while (target.Particles.Count > capacity)
            {
                target.Particles.RemoveLast();
            }

            target.ActiveCount = capacity;
            target.TimeSinceLastSpawn = 0;

            updateCapacity = false;
        }

        private List<ParticleList.Particle> GetNeighbors(ParticleList.Particle target, ParticleList list)
        {
            List<ParticleList.Particle> retval = new List<ParticleList.Particle>();

            foreach (ParticleList.Particle neighbor in list.Particles)
            {
                if (neighbor == target)
                    continue;

                // distance check
                Vector3 displacement = neighbor.Spatial.Position - target.Spatial.Position;
                if (displacement.LengthSq() > rangeSq)
                    continue;

                // angle check
                // NOTE consider +z axis the facing direction
                Vector3 facingDirection = new Vector3(0, 0, 1);
                facingDirection.TransformNormal(target.Spatial.Transform);
                displacement.Normalize();
                if (Vector3.Dot(facingDirection, displacement) < cosAngle)
                    continue;

                retval.Add(neighbor);
            }

            return retval;
        }
    }
}
